#include "robotLac.h"
#include "Servo.h"

char val; //Entrada del bluetooth
robotLac robot(28,1);
int velI, velD;

void setup(){
  Serial3.begin(9600);
  robot.begin();
  robot.detener();
  velI=robot.getVelocidadI();
  velD=robot.getVelocidadD();
}

void loop (){  
  if(Serial3.available()){
    val = Serial3.read();
    Serial3.println(val);
    switch (val){
      case '5': robot.detener(); break;
      case '1': robot.antihorario(); break;
      case '3': robot.horario();     break;
      case '2': robot.avanzar();     break;
      case '8': robot.retroceder();  break;
      case '6': robot.derecha(); break;
      case '4': robot.izquierda(); break;
      case '/': velD = velD + 5; robot.setVelocidadD(velD); break;
      case '*': velI = velI + 5; robot.setVelocidadI(velI); break;
      case '=': velD = velD - 5; robot.setVelocidadD(velD); break;
      case ',': velI = velI - 5; robot.setVelocidadI(velI); break;
      case '0':
              Serial3.println("Estado del robot");
              Serial3.print("Movimiento: ");
              Serial3.println(robot.getEstado());
              Serial3.print("Velocidad Derecha: ");
              Serial3.println(robot.getVelocidadD());
              Serial3.print("Velocidad Izquierda: ");
              Serial3.println(robot.getVelocidadI());
              break;
      default:  robot.detener(); break;
      delay(100);
    }
  }
}
